#include "MathLib.h"
#include "kniBase.h"

#include "./KNI_InvKin/ikBase.h"
#include "./KNI/kmlMotBase.h"
#include "./common/MathHelperFunctions.h"

#include "getch.h"
#include <pthread.h>


// katanaG parameters 
#define CFGFILE_G "/opt/KNI_3.9.1/configfiles400/katana6M90G.cfg"
#define IPADDR_G "192.168.168.231"
#define PORTNB_G 5566  
#define MotorMax_G 6

// katanaT parameters
#define CFGFILE_T "/opt/KNI_3.9.1/configfiles400/katana6M90T.cfg"
#define IPADDR_T "192.168.168.232"
#define PORTNB_T 5566  
#define MotorMax 6


/**
 * threaded Spline replay class 
 *
 * example use : 
 * \code
 * 
 *   SplineMovement sp(move,katana);
 *   sp.initMovement();
 *   std::cout << "press space to start movement" << std::endl;
 *   c=0;
 *   while(c!=' ')
 *     c = getch();
 *   sp.playMovement();
 *
 *  std::cout << "press a space to switch off the robot .. " << std::endl;
 *  c=0;
 *  while(c!=' ')
 *    c = getch();
 *  katana->switchRobotOff();
 * \endcode
 *
 *
 * move is a matrix, first columns : time stamps (ms), other columns
 * joint angles in *radians*. katana is a CLMBase intialised Object pointer 
 */

class SplineMovement {
public :

  // all those fields should be protected .. 

  short *** parameters; // raw splines parameters .. (should be protected ..) 
  short * initialPosition;  
  int steps; 
  int nMotors;
  int currentstep;
  int finished;
  int time_per_step; /* 1 := 10 ms  : this is the time for 1 spline step, 
			50 (means 500ms) is a good default value */
  struct timeval begin_time;
  CLMBase * katana;
  const TKatMOT* mot;
  KNI::KinematicParameters joint;
  pthread_t mThread;
  
  // end protected fields .. 

  /**
   * got the joint angles values after the replay
   */

  Matrix feedbackMatrix; // get encoders values in this matrix

  /**
   * Creator 
   * \param in Matrix containing the desired trajectory. 
   * - first column : time (ms) 
   * - 2nd column : 1st joint angles values
   * - ... 
   */

  SplineMovement(Matrix& in,CLMBase * katana);

  /**
   * called by default creator
   */
  void GetFromMatrix(Matrix& in);
  
  
  /**
   * send one spline step to robot 
   */
  int sendSplineStep();
  
  /**
   * to call before starting a movement 
   */
  void initMovement();
  
  /**
   * start a thread and replay the movement 
   */
  void startMovement_threaded();
  
  /**
   * wait until end of movement 
   */
  void endMovement_threaded();

  /**
   * replay function (intended to be lanch in a separate thread
   */
  void playMovement(void);  
  
  /**
   * stop the reproduction (not very safe ... )
   */
  void freeze(void);

  ~SplineMovement() {
    for (int i = 0; i < steps; i++)
      for (int j = 0; j < nMotors; j++)
	delete [] parameters[i][j];
    for (int i = 0; i < steps; i++)
      delete [] parameters[i];
    delete [] parameters;
  };

   private :
      int valCount;
      int ** feedbackValues;
    

};

extern "C" void  * runThread(void *); // C prototype to allow a call from pthread_create() 


void getEncodersValues(CLMBase * katana,int * vals,struct timeval begin);

void getAnglesValues(CLMBase * katana,Vector& values);

void encodersToAngle(CLMBase * katana,Vector& encoders, Vector& angles);

Matrix encodersToMatrix(CLMBase * katana,int ** vals,int nValues);

Matrix demonstration(CLMBase * katana); // grab a demo from katana and store to a matrix object

void moveToAnglesValues(CLMBase * katana,Vector& values);

void controle_moteur_vitesse(CLMBase *katana,int idx, int velocity);
void controle_moteur_vitesse_rads(CLMBase *katana,
				  int m, /* motor index */
				  float velocity); /* velocity in rad/s */

// initialise the two katanas .. 

CLMBase * initKatanaG();

CLMBase * initKatanaT();
